#include "coordinate.h"

void A_to_B_straight(rectangle *squar, int x1, int x2, int y1, int y2, int edge)
{
    float dx = (float)(x2 - x1) / (float)squar->step;
    float dy = (float)(y2 - y1) / (float)squar->step;
    // squar->move_x[edge - 1][0] = (float)x1;
    // squar->move_y[edge - 1][0] = (float)y1;
    for (int j = 0; j < 10; j++)
    {
        squar->move_x[edge - 1][j] = x1;
        squar->move_y[edge - 1][j] = y1;
    }
    for (int i = 10; i < (squar->step + 10); i++)
    {
        squar->move_x[edge - 1][i] = squar->move_x[edge - 1][i - 1] + dx;
        squar->move_y[edge - 1][i] = squar->move_y[edge - 1][i - 1] + dy;
    }
    for (int k = 40; k < 50; k++)
    {
        squar->move_x[edge - 1][k] = x2;
        squar->move_y[edge - 1][k] = y2;
    }
}

void Planning_route(rectangle *squar)
{
    /* A to B */
    A_to_B_straight(squar, squar->A[0], squar->B[0], squar->A[1], squar->B[1], 1);
    /* B to C */
    A_to_B_straight(squar, squar->B[0], squar->C[0], squar->B[1], squar->C[1], 2);
    /* C to D */
    A_to_B_straight(squar, squar->C[0], squar->D[0], squar->C[1], squar->D[1], 3);
    /* D to A */
    A_to_B_straight(squar, squar->D[0], squar->A[0], squar->D[1], squar->A[1], 4);

    // /* A to B */
    // A_to_B_straight(squar, squar->A[0], squar->D[0], squar->A[1], squar->D[1], 1);
    // /* B to C */
    // A_to_B_straight(squar, squar->D[0], squar->C[0], squar->D[1], squar->C[1], 2);
    // /* C to D */
    // A_to_B_straight(squar, squar->C[0], squar->B[0], squar->C[1], squar->B[1], 3);
    // /* D to A */
    // A_to_B_straight(squar, squar->B[0], squar->A[0], squar->B[1], squar->A[1], 4);

    squar->receive_flag = 1;
}
